/*
* Skel.cpp
*
*  Created on: 2012-6-17
*      Author: shenchen
*/

    #include <stdlib.h>
    #include <iostream>
    #include <vector>

    #include <XnCppWrapper.h>
    #include <XnModuleCppInterface.h>
    #include "opencv/cv.h"
    #include "opencv/highgui.h"

    using namespace std;
    using namespace cv;

void CheckOpenNIError(XnStatus eRes, String status){
	if (eRes != XN_STATUS_OK)
		cerr << status << "Error: " << xnGetStatusString (eRes) << endl;
}

int main(int argc, char** arvg){
	XnStatus eRes =XN_STATUS_OK; // "eRes" means the status during Kinect work
	xn::ImageMetaData MetaImage;
	xn::DepthMetaData MetaDepth;

	// Initialize OpenCV
	cvNamedWindow("Image Map",1);
	cvNamedWindow("Depth Map",1);
	char key=0;
	IplImage* imgDepth16u=cvCreateImage(cvSize(640,480),IPL_DEPTH_16U,1);
	IplImage* imgRGB8u=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
	IplImage* depthShow=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
	IplImage* imageShow=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);

	// initialize context
	xn::Context xContext;
	eRes = xContext.Init();
	CheckOpenNIError(eRes, "INITIALIZE CONTEXT");

	// Create the image/depth generators
	xn::ImageGenerator ImgGenerator;
	eRes = ImgGenerator.Create(xContext);
	CheckOpenNIError(eRes, "Successfully in creating image generator");
	xn::DepthGenerator DptGenerator;
	eRes = DptGenerator.Create(xContext);
	CheckOpenNIError(eRes, "Successfully in creating depth generator");

	// setting standard map mode
	XnMapOutputMode mapMode;
	mapMode.nXRes = 640;
	mapMode.nYRes = 480;
	mapMode.nFPS = 30;
	eRes = ImgGenerator.SetMapOutputMode( mapMode );
	eRes = DptGenerator.SetMapOutputMode( mapMode );

	// correct the view point
	DptGenerator.GetAlternativeViewPointCap().SetViewPoint( ImgGenerator );

	// read the data
	eRes = xContext.StartGeneratingAll();
	eRes = xContext.WaitNoneUpdateAll();

	while(!(eRes=xContext.WaitNoneUpdateAll()) && (key!=27)){
		// get the meta data
		ImgGenerator.GetMetaData(MetaImage);
		DptGenerator.GetMetaData(MetaDepth);
		//OpenCV output
		memcpy(imgDepth16u->imageData,MetaDepth.Data(),640*480*2);
		cvConvertScale(imgDepth16u,depthShow,255/4096.0,0);
		memcpy(imgRGB8u->imageData,MetaImage.Data(),640*480*3);
		cvCvtColor(imgRGB8u,imageShow,CV_RGB2BGR);
		cvShowImage("Image Map",imageShow);
		cvShowImage("Depth Map",depthShow);
		key=cvWaitKey(20);
	}

	// destroy openCV
	cvDestroyWindow("depth");
	cvDestroyWindow("image");
	cvReleaseImage(&imgDepth16u);
	cvReleaseImage(&imgRGB8u);
	cvReleaseImage(&depthShow);
	cvReleaseImage(&imageShow);
	xContext.StopGeneratingAll();
	xContext.Shutdown();
	return 0;
}
